package hcqs.nav.grid.map.algo;

import hcqs.nav.grid.consts.GridMapConsts;
import hcqs.nav.grid.map.api.IGridMapCompressTool;

public class GridMapCompressTool implements IGridMapCompressTool {
    @Override
    public byte[][] extract(int row, int col, byte[] data) throws Exception {
//        System.out.println(String.format("data count : %s", data.length));
        byte[][] result = new byte[row][col];
        ArrayPose cp = new ArrayPose(row, col);
//        int count = 0;
        for (byte b : data) {
//            count++;
//            System.out.println(String.format("%s,", byteToBit(b)));
            byte b1 = (byte) ((b & 0b11000000) >> 6);
            byte b2 = (byte) ((b & 0b00110000) >> 4);
            byte b3 = (byte) ((b & 0b00001100) >> 2);
            byte b4 = (byte) (b & 0b00000011);
            result[cp.curRowIndex][cp.curColIndex] = b1;
            if (cp.nextPose())
                result[cp.curRowIndex][cp.curColIndex] = b2;
            if (cp.nextPose())
                result[cp.curRowIndex][cp.curColIndex] = b3;
            if (cp.nextPose())
                result[cp.curRowIndex][cp.curColIndex] = b4;
            if (!cp.nextPose())
                break;
        }
//        System.out.println(String.format("data count : %s", count));
        return result;
    }

    @Override
    public byte[] compress(byte[][] data) throws Exception {
        if (data == null || data.length == 0)
            return null;
        int size = data.length * data[0].length / 4 + ((data.length * data[0].length) % 4 > 0 ? 1 : 0);
        byte[] result = new byte[size];
        for (int row = 0, curPos = 0; row < data.length; row++, curPos++) {
            for (int col = 0; col < data[row].length; col++) {
                int left = (3 - curPos % 4) * 2;
                if (data[row][col] == GridMapConsts.GRID_PASS) {
                    result[curPos / 4] |= (byte) ((0b00) << left);
                } else if (data[row][col] == GridMapConsts.GRID_OBSTACLE) {
                    result[curPos / 4] |= (byte) ((0b01) << left);
                } else {
                    result[curPos / 4] |= (byte) ((0b10) << left);
                }
            }
        }
        return result;
    }

    public static String byteToBit(byte b) {
        return ""
                + (byte) ((b >> 7) & 0x1) + (byte) ((b >> 6) & 0x1)
                + (byte) ((b >> 5) & 0x1) + (byte) ((b >> 4) & 0x1)
                + (byte) ((b >> 3) & 0x1) + (byte) ((b >> 2) & 0x1)
                + (byte) ((b >> 1) & 0x1) + (byte) ((b >> 0) & 0x1);
    }

    public static void main(String[] args) {
        byte[] data = new byte[]{
                (byte) 0b10101010,
                (byte) 0b10101010,
                (byte) 0b10101010,
                (byte) 0b10101010,
        };
//
//        GridMapCompressTool gct = new GridMapCompressTool();
//        try {
//            byte[][] result = gct.extract(100, 10, data);
//            for (int row = 0; row < result.length; row++) {
//                for (int col = 0; col < result[row].length; col++) {
//                    System.out.print(String.format("%d,", result[row][col]));
//                }
//                System.out.println();
//            }
//        } catch (Exception e) {
//
//        }

//        byte b = (byte)0b10000000;
//        System.out.println((b & 0b11000000) >> 6);
//        GridMapCompressTool.ArrayPose ap = new GridMapCompressTool.ArrayPose(3, 3);
//        ap.nextPose();
//        System.out.println(ap.curColIndex);
        GridMapCompressTool gct = new GridMapCompressTool();
        try {
            byte[][] result = gct.extract(100, 40, data);
            for (int row = 0; row < result.length; row++) {
                for (int col = 0; col < result[row].length; col++) {
                    System.out.print(String.format("%d,", result[row][col]));
                }
                System.out.println();
            }
//            System.out.println(String.format("%d", result[99][9]));
        } catch (Exception e) {

        }

    }

    static class ArrayPose {
        public ArrayPose(int row, int col) {
            this.row = row;
            this.col = col;
        }

        public int row = 0;
        public int col = 0;
        public int curRowIndex = 0;
        public int curColIndex = 0;

        public boolean nextPose() {
            if (++this.curColIndex >= col) {
                this.curColIndex = 0;
                this.curRowIndex++;
            }
            return curRowIndex < row;
        }
    }
}
